//
// Created by xujingyi on 2021/5/19.
//
#include <math.h>
#include <iostream>
#include "solve/trajectory_resolve.h"
#include "options.h"
#include "Setter.h"
#include "Serial.h"
#include "log.h"

extern cv::Mat show_pic;
extern rm::Setter rm_setter;
extern Serial serial;

#if SHOOTER_MODE==UP_SHOOTER
float rm::TrajectoryResolve::getFireAngle(const float &bullet_spd, const float &pitch,const float dis){
        float distance=dis*0.1;
//        char ch[100];
        float h1=distance/(cos(pitch/ANGLE_PER_RAD))*sin((serial.qPitches[-1]+pitch)/ANGLE_PER_RAD);
        //        float armor_h=rm_setter.sentry_height - 145*sin((PITCH_ANGLE_PLUS*(serial.rxpackage.pitch_angle.type_short-PITCH_HORIZON_ANGLE)/10.f)/ANGLE_PER_RAD)-h1;
        float v = 1000*((bullet_spd*0.1f)+20)*0.4f;
        float th = (serial.qPitches[-1] )/(ANGLE_PER_RAD);
//        float Ta = tan(th);
//        float Co = cos(th);
//        float Si = sin(th);

        float Wh;
        float h = dis*sin(th);/*1015.0f-150-185.0f*Si*/;
        Wh = 0.5*9.8*1000*dis*sqrtf(dis*dis-h*h)/(v*v)+105;//-35;// mm
        Wh = Wh/dis*179.0f;
        float dwy;
    ////dwy:正直往上，负值往下
        dwy = Wh+pow((dis/1000),2)*rm_setter.pitch_gravity_plus_coefficient;//-pow(((z+setter.camera2ptz.at<camera2ptztype>(2))-1300)/300.f,2)*2;//+5
//    std::cout<<"Wh"<<Wh<<"dwy:"<<dwy<<std::endl;
        if((dwy)>30){
            dwy = Wh+pow((dis/1000),2.0)*(rm_setter.pitch_gravity_plus_coefficient+0.3);//-pow(((z+setter.camera2ptz.at<camera2ptztype>(2))-1300)/300.f,2)*2;//+5
        }
        else if((dwy)<-29){
                dwy = -29;
            }
//std::cout<<"Wh"<<Wh<<"dwy:"<<dwy<<std::endl;
        float temp_pitch=-(float)atan(dwy/(FOCUS_PIXAL*FOCUS))*ANGLE_PER_RAD;
        return -dwy;
};

float rm::TrajectoryResolve::getFireAngle_highland(const float &bullet_spd, const float &pitch,const float dis){
//    std::cerr<<"HIGHLAND MODE"<<std::endl;
    float distance=dis*0.1;
    char ch[100];
    float h1=distance/(cos(pitch/ANGLE_PER_RAD))*sin((serial.qPitches[-1]+pitch)/ANGLE_PER_RAD);
    float v = 1000*((bullet_spd*0.1f)+20)*0.4f;
    float th = serial.qPitches[-1]/(ANGLE_PER_RAD);
//        float Ta = tan(th);
//        float Co = cos(th);
//        float Si = sin(th);

    float Wh;
    float h = dis*sin(th);/*1015.0f-150-185.0f*Si*/;
    Wh = 0.5*9.8*1000*dis*sqrtf(dis*dis-h*h)/(v*v)+95;//-35;// mm
    Wh = Wh/dis*179.0f;
    float dwy;
    ////dwy:正直往上，负值往下
    dwy = Wh+pow((dis/1000),2)*rm_setter.highland_pitch_gravity_plus_coefficient;//-pow(((z+setter.camera2ptz.at<camera2ptztype>(2))-1300)/300.f,2)*2;//+5
    if((dwy)>75){
        dwy = Wh+pow((dis/1000),1.0)*(rm_setter.highland_pitch_gravity_plus_coefficient-0.57);//-pow(((z+setter.camera2ptz.at<camera2ptztype>(2))-1300)/300.f,2)*2;//+5
    }
    if((dwy)<10){
        dwy = -5;
    }
    if((dwy)>200){
        dwy = 200;//-pow(((z+setter.camera2ptz.at<camera2ptztype>(2))-1300)/300.f,2)*2;//+5
    }
    float temp_pitch=-(float)atan(dwy/(FOCUS_PIXAL*FOCUS))*ANGLE_PER_RAD;
    return -dwy;
}
#elif SHOOTER_MODE==DOWN_SHOOTER
float rm::TrajectoryResolve::getFireAngle(const float &bullet_spd, const float &pitch,const float dis){
        float distance=dis*0.1;
//        char ch[100];
        float h1=distance/(cos(pitch/ANGLE_PER_RAD))*sin((serial.qPitches[-1]+pitch)/ANGLE_PER_RAD);
        //        float armor_h=rm_setter.sentry_height - 145*sin((PITCH_ANGLE_PLUS*(serial.rxpackage.pitch_angle.type_short-PITCH_HORIZON_ANGLE)/10.f)/ANGLE_PER_RAD)-h1;
        float v = 1000*((bullet_spd*0.1f)+20)*0.4f;
        float th = (serial.qPitches[-1] )/(ANGLE_PER_RAD);
//        float Ta = tan(th);
//        float Co = cos(th);
//        float Si = sin(th);

        float Wh;
        float h = dis*sin(th);/*1015.0f-150-185.0f*Si*/;
        Wh = 0.5*9.8*1000*dis*sqrtf(dis*dis-h*h)/(v*v)-105;//-35;// mm
        Wh = Wh/dis*179.0f;
        float dwy;
    ////dwy:正直往上，负值往下
//        dwy = Wh+pow((dis/1000),1.3)*rm_setter.pitch_gravity_plus_coefficient;//-pow(((z+setter.camera2ptz.at<camera2ptztype>(2))-1300)/300.f,2)*2;//+5
    dwy=Wh;
//        std::cout<<"Wh"<<Wh<<"dwy:"<<dwy<<std::endl;
    if((dwy)>125){
        dwy = 73;
        return -dwy;
    }
//        if((dwy)<35){
//            dwy = Wh+pow((dis/1000),1.7)*(rm_setter.pitch_gravity_plus_coefficient);//-pow(((z+setter.camera2ptz.at<camera2ptztype>(2))-1300)/300.f,2)*2;//+5
//        }
//std::cout<<"dwy:"<<dwy<<std::endl;
        float temp_pitch=-(float)atan(dwy/(FOCUS_PIXAL*FOCUS))*ANGLE_PER_RAD;
        return -dwy;
};

float rm::TrajectoryResolve::getFireAngle_highland(const float &bullet_spd, const float &pitch,const float dis){
    float distance=dis*0.1;
//        char ch[100];
    float h1=distance/(cos(pitch/ANGLE_PER_RAD))*sin((serial.qPitches[-1]+pitch)/ANGLE_PER_RAD);
    //        float armor_h=rm_setter.sentry_height - 145*sin((PITCH_ANGLE_PLUS*(serial.rxpackage.pitch_angle.type_short-PITCH_HORIZON_ANGLE)/10.f)/ANGLE_PER_RAD)-h1;
    float v = 1000*((bullet_spd*0.1f)+20)*0.4f;
    float th = (serial.qPitches[-1] )/(ANGLE_PER_RAD);
//        float Ta = tan(th);
//        float Co = cos(th);
//        float Si = sin(th);

    float Wh;
    float h = dis*sin(th);/*1015.0f-150-185.0f*Si*/;
    Wh = 0.5*9.8*1000*dis*sqrtf(dis*dis-h*h)/(v*v)-105;//-35;// mm
    Wh = Wh/dis*179.0f;
    float dwy;
    ////dwy:正直往上，负值往下
//        dwy = Wh+pow((dis/1000),1.3)*rm_setter.pitch_gravity_plus_coefficient;//-pow(((z+setter.camera2ptz.at<camera2ptztype>(2))-1300)/300.f,2)*2;//+5
    dwy=Wh+7;
    if((dwy)>125){
        dwy = 73;
        return -dwy;
    }
//        if((dwy)<35){
//            dwy = Wh+pow((dis/1000),1.7)*(rm_setter.pitch_gravity_plus_coefficient);//-pow(((z+setter.camera2ptz.at<camera2ptztype>(2))-1300)/300.f,2)*2;//+5
//        }
//std::cout<<"dwy:"<<dwy<<std::endl;
    float temp_pitch=-(float)atan(dwy/(FOCUS_PIXAL*FOCUS))*ANGLE_PER_RAD;
    return -dwy;
}
#endif


extern cv::Mat show_pic;
extern int last_fps;
float rm::TrajectoryResolve::getMoveCoeffAngle(const float speed,const float pixel_yaw,const float PTZ_yaw,const float dis){
    if(abs(speed)<MYEPS) return 0;
    float vt=speed*(1.0f/float(last_fps)+0.01);//average_fps:110
    float x1=dis*sin((PTZ_yaw+pixel_yaw)/ANGLE_PER_RAD);
    float v_dis=dis*cos((PTZ_yaw+pixel_yaw)/ANGLE_PER_RAD);
    float delta_angle=atan((x1+vt)/v_dis)*ANGLE_PER_RAD-(PTZ_yaw+pixel_yaw);
    if(serial.rxpackage.outpost_alive==0) if(abs(speed)<rm_setter.not_shoot_chassis_speed) delta_angle=0;
    float delta_yaw=delta_angle*rm_setter.move_angle_plus_coefficient;
    LOGM("vt:%f,x1:%f,v_dis:%f,delta_angle:%f,yaw:%f",vt,x1,v_dis,delta_angle,delta_yaw);
    char cyaw[200],cspeed[200],cdelta_angle[200];
    sprintf(cyaw,"ptz_yaw:%.2f",float(PTZ_yaw));
    cv::putText(show_pic,cyaw,cv::Point(50,100),2,1,cv::Scalar(0,255,0),1);
    sprintf(cspeed,"chassis_speed%.2f",float(serial.rxpackage.chassis_speed.type_short));
    cv::putText(show_pic,cspeed,cv::Point(50,150),2,1,cv::Scalar(0,255,0),1);
    sprintf(cdelta_angle,"cdelta_angle%.10f",float(delta_yaw));
    cv::putText(show_pic,cdelta_angle,cv::Point(50,200),2,1,cv::Scalar(0,255,0),1);
    return delta_yaw;
}
